package edu.csusm.spotrobot;

import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.sensorboard.io.IScalarInput;
import java.io.IOException;
import java.util.Vector;

/**
 * Runs on a thread and constantly gets new readings from the IR sensors.  The
 * IR sensors are very prone to noise and give bad readings in between the good
 * readings.  To cope with this IrSensor takes 100 readings as fast as possible,
 * sorts the array of readings then averages the top 70 readings.  Originally
 * designed for four IR sensors (two on the left and two on the right,) but Earl
 * only uses two IR sensors on the left.
 *
 * @author Zebulon Evans
 * @author Clark Scheff
 */
public class IrSensor extends Thread {
    /** The total number of IR sensors */
    public static final int NUMSENSORS        = 4;
    private EDemoBoard demo = EDemoBoard.getInstance();
    /** array to hold the readings from the analog IR sensors */
    private IScalarInput analogReadings[];
    /** a Vector representing the readings for each of the four sensors */
    private Vector sensors[] = new Vector[4];
    /** the last reading from the IR sensors */
    private float lastReading[] = {2.0f, 2.0f, 2.0f, 2.0f};
    /** 
     * Used to calibrate the Sharp IR sensors.  If you look at the data sheet
     * you will notice that we can get a line representing the readings. These 
     * are the values obtained from taking multiple readings from multiple 
     * distances with each sensors and used to calibrate the IR sensors.  Each
     * entry 0 is for the first IR, 1 for the second and so on.
     */
    private double a[] = {0.000221533, 1.0, 0.000214074, 1.0};
    /** 
     * Used to calibrate the Sharp IR sensors.  If you look at the data sheet
     * you will notice that we can get a line representing the readings. These 
     * are the values obtained from taking multiple readings from multiple 
     * distances with each sensors and used to calibrate the IR sensors.  Each
     * entry 0 is for the first IR, 1 for the second and so on.
     */
    private double b[] = {-0.000159924, 1.0, -0.000252226, 1.0};
    
    /* 
     * Constructor to create a new IrSensor class to handle the IR sensors.
     */
    public IrSensor(  ){
        //TODO initialization needs to happen better
        sensors[0] = new Vector();
        sensors[1] = new Vector();
        sensors[2] = new Vector();
        sensors[3] = new Vector();
    }
    
    /**
     * Runs for the lifetime of the thread taking new averaged readings. Takes
     * 100 readings for each sensor, sorts the readings, averages the upper 70
     * to try to get a good value for the distance from the Sharp IR sensor.
     */
    public void run() {
        try{
            while(true){
                
                int averagedReading;
                
                // get 100 readings for all the analog IR Sensors
 
                int[][] readings = new int[4][100];
                for( int reading = 0; reading < 100; reading++ ){
                    analogReadings = demo.getScalarInputs();
                    for( int sensor = 0; sensor < NUMSENSORS; sensor++){
                        readings[sensor][reading] = analogReadings[sensor].getValue();
                    }  
                }

                averagedReading = 0;
                for( int sensor = 0; sensor < NUMSENSORS; sensor++){
                    com.sun.squawk.util.Arrays.sort(readings[sensor]);
                    for( int i = 30; i < 100; i++ ){
                        averagedReading += readings[sensor][i];
                    }
                    averagedReading = averagedReading / 70;
                    newReading(sensor, (float)( averagedReading ) );
                }
                
                Thread.sleep(100);
            }
        }catch(Exception e){}
    }
    
    /**
     * Get a reading from one of the IR sensors.
     *
     * @param int indicating which IR sensor to get a reading from.
     * @return float the distance that the IR sensor is from an object.
     */
    public float getReading(int z){
          float voltage =  ((Float)sensors[z].elementAt(3)).floatValue();
          return voltageToDistance( voltage, a[z], b[z] );
    }
    
    /**
     * Adds a new reading to the correct sensor vector.  We keep a few old 
     * readings around in case we need them to filter out bad values.
     *
     * @param int the sensor that the reading is from
     * @param float the raw adc converted reading from the sensor
     */
    private void newReading( int i, float reading ){
        // initialize the vectors
        // TODO: put this in a constructor
        float alpha = 0.3f;
        if( sensors[i].size() < 4 ){
            for( int z = 0; z < 4; z++ ){
                sensors[i].addElement(new Float(reading));
            }
        }
        
        lastReading[i] = reading;
        sensors[i].addElement(new Float(reading));
        sensors[i].removeElementAt(0);     
    }
    
    /**
     * Converts the raw ADC reading from the sensor to a distance using the a
     * and b arrays that we use for the conversion.  The a and b values were
     * obtained by noticing that we can get a line as on the data sheet, so we
     * plotted a bunch of values to calibrate the sensors and got the a and b 
     * values for the line.
     *
     * @param float the raw ADC reading for the IR sensor to convert 
     * @param double the a param to the line equation
     * @param double the b param to the line equation
     */
    private float voltageToDistance( float voltage, double aIn, double bIn ){
        // TODO change these to ints, raw adc reading of voltage really is an int
        return (float)( 1/(aIn * (double)voltage + bIn) - 0.42 );  
    }

}
